#include "camera.hpp"

Camera::Camera(std::shared_ptr<ros::NodeHandle> node_handle_ptr,
               const std::string &serial_port)
    : node_handle_ptr_(node_handle_ptr), Serial(serial_port) {
    name_ = serial_port.substr(5);
    //   publisher_ = node_handle_ptr_->advertise<sensor_msgs::Camera>(name_,
    //   1);
    switch_ = node_handle_ptr_->advertiseService(name_, &Camera::SwitchCallBack,
                                                 this);
    //   LOG(ERROR) << name_ << " camera frame size:(" << sizeof(ImuFrame) <<
    //   ")";

    if (AS_SDK_Init() != 0)
        LOG(ERROR) << "Angstrong sdk initial failed.";
    else
        LOG(ERROR) << "Angstrong sdk initial succeeded.";

    char sdk_version[64] = {0};
    if (AS_SDK_GetSwVersion(sdk_version, sizeof(sdk_version)) != 0)
        LOG(ERROR) << "Angstrong get sdk version failed.";
    else
        LOG(ERROR) << "Angstrong get sdk version (" << sdk_version << ").";
}

Camera::~Camera() {
    if (AS_SDK_Deinit() != 0)
        LOG(ERROR) << "Angstrong sdk destory failed.";
    else
        LOG(ERROR) << "Angstrong sdk destory succeeded.";
}

void Camera::CameraRxThread() {
    ros::Rate rate(kCameraFrequence);
    std::vector<uint8_t> rx_data;
    // LOG(ERROR) << name_ << " thread started!";
    // int astra_device_lock_shm_id =
    //     shm_open("astra_device.lock", O_RDWR | O_CREAT, 0777);
    // int ret = ftruncate(astra_device_lock_shm_id, 4);
    // uint8_t *astra_device_lock_shm_ptr = nullptr;
    // astra_device_lock_shm_ptr =
    //     (uint8_t *)mmap(nullptr, 4, PROT_READ | PROT_WRITE, MAP_SHARED,
    //                     astra_device_lock_shm_id, 0);
    // pthread_mutexattr_t astra_device_lock_attr;
    // pthread_mutexattr_init(&astra_device_lock_attr);
    // pthread_mutexattr_setpshared(&astra_device_lock_attr,
    // PTHREAD_PROCESS_SHARED); pthread_mutex_t *astra_device_lock = nullptr;
    // astra_device_lock = (pthread_mutex_t *)astra_device_lock_shm_ptr;
    // pthread_mutex_init(astra_device_lock, &astra_device_lock_attr);

    // openni::OpenNI::setLogMinSeverity(1);
    // openni::OpenNI::setLogConsoleOutput(true);
    // openni::OpenNI::setLogFileOutput(true);
    // auto rc = openni::OpenNI::initialize();
    // if (rc != openni::STATUS_OK)
    //   LOG(ERROR) << "initialize rc not ok!!!!!!!!!";
    // else
    //   LOG(ERROR) << "initialize rc ok!!!!!!!!!";
    // sleep(1);

    // std::vector<openni::DeviceInfo> device_list;
    // openni::Array<openni::DeviceInfo> device_info_list;
    // openni::OpenNI::enumerateDevices(&device_info_list);
    // LOG(ERROR) << "Found " << device_info_list.getSize() << " devices";

    while (true) {
        if (is_running_ && ReadFromIO(rx_data))
            ParseData(rx_data);
        rate.sleep();
    }
}

void Camera::ParseData(std::vector<uint8_t> &data) {
    LOG(ERROR) << "size:(" << data.size() << ")";
    //   auto get_frame_crc = [&](const std::vector<uint8_t> &data) {
    //     uint16_t summary = 0;
    //     if (data.size() != sizeof(ImuFrame)) return summary;
    //     for (size_t i = 1; i < sizeof(ImuFrame) - 4; i++) summary += data[i];
    //     return summary;
    //   };

    //   while (data.size() >= sizeof(ImuFrame)) {
    //     size_t drop = 0;
    //     while (data.size() >= sizeof(ImuFrame)) {
    //       if (data[0] == kImuHeader && data[sizeof(ImuFrame) - 2] == kImuEnd1
    //       &&
    //           data[sizeof(ImuFrame) - 1] == kImuEnd2) {
    //         size_t length = size_t(uint16_t(data[6]) << 8 |
    //         uint16_t(data[5])); if (length != 92)
    //           LOG(ERROR) << "length: (" << length << "),sizeof frame:("
    //                      << sizeof(ImuFrame) << ")";
    //         break;
    //       } else {
    //         drop++;
    //         data.erase(data.begin());
    //       }
    //     };
    //     if (drop > 0) LOG(ERROR) << name_ << " drop size:(" << drop << ").";
    //     if (data.size() < sizeof(ImuFrame)) return;
    //     std::vector<uint8_t> imu_data(data.begin(),
    //                                   data.begin() + sizeof(ImuFrame));
    //     data.erase(data.begin(), data.begin() + sizeof(ImuFrame));
    //     ImuFrame *imu_frame = reinterpret_cast<ImuFrame *>(imu_data.data());
    //     if (get_frame_crc(imu_data) != imu_frame->crc) {
    //       LOG(ERROR) << name_ << " frame crc error.";
    //       return;
    //     }
    //     // LOG(ERROR) << "euler_angle:(" << imu_frame->euler_angle[0] << ","
    //     //            << imu_frame->euler_angle[1] << ","
    //     //            << imu_frame->euler_angle[2] << ").";
    //     sensor_msgs::Camera camera;
    //     camera.header.stamp = ros::Time::now();
    //     camera.header.frame_id = name_;
    //     camera.linear_acceleration.x = imu_frame->corrected_accel[0];
    //     camera.linear_acceleration.y = imu_frame->corrected_accel[1];
    //     camera.linear_acceleration.z = imu_frame->corrected_accel[2];
    //     camera.linear_acceleration_covariance = {0.04, 0, 0, 0, 0.04, 0, 0,
    //     0, 0.04}; camera.angular_velocity.x = imu_frame->corrected_gyro[0];
    //     camera.angular_velocity.y = imu_frame->corrected_gyro[1];
    //     camera.angular_velocity.z = imu_frame->corrected_gyro[2];
    //     camera.angular_velocity_covariance = {0.02, 0, 0, 0, 0.02, 0, 0, 0,
    //     0.02};
    //     camera.orientation.x = imu_frame->quaternion[0];
    //     camera.orientation.y = imu_frame->quaternion[1];
    //     camera.orientation.z = imu_frame->quaternion[2];
    //     camera.orientation.w = imu_frame->quaternion[3];
    //     camera.orientation_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 0.05};
    //     Publish(camera);
    // };
}

// void Camera::Publish(const sensor_msgs::Camera &camera) {
//   publisher_.publish(camera);
//   static auto last = ros::Time::now();
//   last = camera.header.stamp;
//   times_.push_back(last);
//   while (times_.size() > 100) {
//     times_.erase(times_.begin());
//     LOG_EVERY_N(ERROR, 100)
//         << name_ << " publish frequence in last 100 message:("
//         << double(times_.size() - 1) /
//                ((times_.back() - times_.front()).toSec())
//         << ")hz.";
//   }
// }

bool Camera::SetRunning(const bool &running) {
    // ros::Rate rate(kCameraFrequence);
    LOG(ERROR) << "SetRunning:(" << running << "),is_start:(" << is_start_
               << ")";
    if (running) {
        AS_LISTENER_CALLBACK_S listener_callback = {
            .Attached = Attached,
            .Detached = Detached,
            .privateData = this,
        };
        if (AS_SDK_StartListener(listener_callback, AS_LISTENNER_TYPE_USB,
                                 true) == 0)
            LOG(ERROR) << "Start AS_SDK_StartListener successful.";
        else
            LOG(ERROR) << "Start AS_SDK_StartListener failed.";
        is_running_ = true;
        return true;
    } else {
        if (AS_SDK_StopListener() == 0)
            LOG(ERROR) << "Stop AS_SDK_StartListener successful.";
        else
            LOG(ERROR) << "Stop AS_SDK_StartListener failed.";
        is_running_ = true;
        return true;
    }
}

void Camera::Detached(AS_CAM_ATTR_S *set_attributes_ptr, void *this_ptr) {
    // Camera *camera_ptr = static_cast<Camera *>(this_ptr);
    // if (camera_ptr->device_ptr_ != nullptr) {
    //   if (AS_SDK_GetCameraAttrs(camera_ptr->device_ptr_, set_attributes_ptr)
    //   ==
    //       0) {
    //     LOG(ERROR) << "Get camera attributes successful ,this camera is
    //     exist.";
    //   } else {
    //     LOG(ERROR) << "Get camera mode failed.";
    //   }
    // }
}

// void Camera::Attached(AS_CAM_ATTR_S *set_attributes_ptr, void *this_ptr) {
//     Camera *camera_ptr = static_cast<Camera *>(this_ptr);
//     AS_CAM_PTR device_ptr;
//     AS_CAM_ATTR_S get_attributes_ptr;
//     memset(&get_attributes_ptr, 0, sizeof(AS_CAM_ATTR_S));
//     if (camera_ptr->device_ptr_ != nullptr) {
//         LOG(ERROR) << "Camera_ptr->device_ptr_ already exist.";
//     } else if (AS_SDK_GetCameraAttrs(device_ptr, get_attributes_ptr) == 0) {
//         LOG(ERROR) << "Get camera attributes successful ,this camera is
//         exist."; AS_SDK_CAM_MODEL_E camera_type; if
//         (AS_SDK_GetCameraModel(device_ptr, camera_type) == 0) {
//             LOG(ERROR) << "Get camera mode successful(" << camera_type <<
//             ")."; camera_ptr->device_ptr_ = device_ptr;
//         } else {
//             LOG(ERROR) << "Get camera mode failed.";
//             return;
//         }
//     } else {
//         LOG(ERROR) << "Get camera attributes failed ,this camera is not
//         exist."; if (AS_SDK_CreateCamHandle(device_ptr, set_attributes_ptr)
//         == 0) {
//             LOG(ERROR) << "Create camera handle successful.";
//             AS_SDK_CAM_MODEL_E camera_type = AS_SDK_CAM_MODEL_UNKNOWN;
//             if (AS_SDK_GetCameraModel(device_ptr, camera_type) == 0) {
//                 LOG(ERROR) << "Get camera mode successful(" << camera_type
//                            << ").";
//                 camera_ptr->device_ptr_ = device_ptr;
//             } else {
//                 LOG(ERROR) << "Get camera mode failed.";
//             }
//         } else {
//             LOG(ERROR) << "Create camera handle failed.";
//             return;
//         }
//     }

//     if (AS_SDK_GetCameraAttrs(camera_ptr->device_ptr_, get_attributes_ptr) ==
//         0) {
//         LOG(ERROR)
//             << "Get camera attributes successful while device ptr exist.";
//         LOG(ERROR) << "Camera attributes bnum:("
//                    << get_attributes_ptr.attr.usbAttrs.bnum << ")";
//         LOG(ERROR) << "Camera attributes dnum:("
//                    << get_attributes_ptr.attr.usbAttrs.dnum << ")";
//         LOG(ERROR) << "Camera attributes port_numbers:("
//                    << get_attributes_ptr.attr.usbAttrs.port_numbers << ")";

//         if (AS_SDK_OpenCamera(camera_ptr->device_ptr_,
//                               kCameraConfigFile.c_str()) == 0) {
//             LOG(ERROR) << "Open camera successfully.";
//             AS_CAM_Stream_Cb_s stream_callback;
//             stream_callback.callback = FrameCallback;
//             stream_callback.privateData = this_ptr;
//             if (AS_SDK_RegisterStreamCallback(camera_ptr->device_ptr_,
//                                               &stream_callback) == 0) {
//                 LOG(ERROR) << "Register stream callback successful.";
//                 if (AS_SDK_StartStream(camera_ptr->device_ptr_, false) == 0)
//                 {
//                     LOG(ERROR) << "Camera start stream successful.";
//                 } else {
//                     LOG(ERROR) << "Camera start stream failed.";
//                 }
//             } else {
//                 LOG(ERROR) << "Register stream callback failed.";
//             }
//         } else {
//             LOG(ERROR) << "Open camera failed.";
//         }
//     } else {
//         LOG(ERROR) << "Get camera attributes failed while device ptr exist.";
//     }
// }

void Camera::Attached(AS_CAM_ATTR_S *set_attributes_ptr, void *this_ptr) {
    Camera *camera_ptr = static_cast<Camera *>(this_ptr);
    if (camera_ptr->device_ptr_ != nullptr) {
        LOG(ERROR) << camera_ptr->name_
                   << "Camera_ptr->device_ptr_ already exist.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_ << "Camera_ptr->device_ptr_ not exist.";
    }

    if (camera_ptr->name_ == "camera_up") {
        set_attributes_ptr->attr.usbAttrs.bnum = 3;
        set_attributes_ptr->attr.usbAttrs.dnum = 13;
    } else if (camera_ptr->name_ == "camera_down") {
        set_attributes_ptr->attr.usbAttrs.bnum = 1;
        set_attributes_ptr->attr.usbAttrs.dnum = 5;
    }

    AS_CAM_PTR device_ptr;
    if (AS_SDK_CreateCamHandle(device_ptr, set_attributes_ptr) != 0) {
        LOG(ERROR) << camera_ptr->name_ << "Create camera handle failure.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_ << "Create camera handle successful.";
    }

    AS_SDK_CAM_MODEL_E camera_type = AS_SDK_CAM_MODEL_UNKNOWN;
    if (AS_SDK_GetCameraModel(device_ptr, camera_type) != 0) {
        LOG(ERROR) << camera_ptr->name_ << "Get camera mode failure.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_ << "Get camera mode successful,("
                   << camera_type << ").";
        camera_ptr->device_ptr_ = device_ptr;
    }

    AS_CAM_ATTR_S get_attributes_ptr;
    if (AS_SDK_GetCameraAttrs(camera_ptr->device_ptr_, get_attributes_ptr) !=
        0) {
        LOG(ERROR) << camera_ptr->name_
                   << "Get camera attributes failure while device ptr exist.";
    } else {
        LOG(ERROR)
            << camera_ptr->name_
            << "Get camera attributes successful while device ptr exist.";
        LOG(ERROR) << "Camera attributes vid:(" << std::hex
                   << get_attributes_ptr.attr.usbAttrs.vid << ")";
        LOG(ERROR) << "Camera attributes pid:(" << std::hex
                   << get_attributes_ptr.attr.usbAttrs.pid << ")";
        LOG(ERROR) << "Camera attributes serial:("
                   << get_attributes_ptr.attr.usbAttrs.serial << ")";
        LOG(ERROR) << "Camera attributes bnum:("
                   << get_attributes_ptr.attr.usbAttrs.bnum << ")";
        LOG(ERROR) << "Camera attributes dnum:("
                   << get_attributes_ptr.attr.usbAttrs.dnum << ")";
        LOG(ERROR) << "Camera attributes port_number:("
                   << get_attributes_ptr.attr.usbAttrs.port_number << ")";
        LOG(ERROR) << "Camera attributes port_numbers:("
                   << get_attributes_ptr.attr.usbAttrs.port_numbers << ")";
    }

    if (AS_SDK_OpenCamera(camera_ptr->device_ptr_, kCameraConfigFile.c_str()) !=
        0) {
        LOG(ERROR) << camera_ptr->name_ << "Open camera failure.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_ << "Open camera successfully.";
    }

    AS_CAM_Stream_Cb_s stream_callback;
    stream_callback.callback = FrameCallback;
    stream_callback.privateData = this_ptr;
    if (AS_SDK_RegisterStreamCallback(camera_ptr->device_ptr_,
                                      &stream_callback) != 0) {
        LOG(ERROR) << camera_ptr->name_ << "Register stream callback failure.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_
                   << "Register stream callback successful.";
    }

    if (AS_SDK_GetCamParameter(camera_ptr->device_ptr_,
                               &camera_ptr->parameter_) != 0) {
        LOG(ERROR) << camera_ptr->name_ << "Get camera parameter failure.";
    } else {
        LOG(ERROR) << camera_ptr->name_ << "Get camera parameter successful.";
        LOG(ERROR) << camera_ptr->name_ << "parameter.fxir:("
                   << camera_ptr->parameter_.fxir << ");";
        LOG(ERROR) << camera_ptr->name_ << "parameter.fyir:("
                   << camera_ptr->parameter_.fyir << ");";
        LOG(ERROR) << camera_ptr->name_ << "parameter.cxir:("
                   << camera_ptr->parameter_.cxir << ");";
        LOG(ERROR) << camera_ptr->name_ << "parameter.cyir:("
                   << camera_ptr->parameter_.cyir << ");";
    }

    if (AS_SDK_StartStream(camera_ptr->device_ptr_, false) != 0) {
        LOG(ERROR) << camera_ptr->name_ << "Camera start stream failed.";
        return;
    } else {
        LOG(ERROR) << camera_ptr->name_ << " camera start stream successful.";
    }
}

void Camera::FrameCallback(AS_CAM_PTR device_ptr,
                           const AS_SDK_Data_s *sdk_data_ptr, void *this_ptr) {
    Camera *camera_ptr = static_cast<Camera *>(this_ptr);
    // LOG(ERROR) << camera_ptr->name_ << " frame size:("
    //            << sdk_data_ptr->depthImg.size << "),width:("
    //            << sdk_data_ptr->depthImg.width << "),height:("
    //            << sdk_data_ptr->depthImg.height << "),window:("
    //            << sdk_data_ptr->depthImg.width *
    //            sdk_data_ptr->depthImg.height
    //            << "),size:(" << sdk_data_ptr->depthImg.size << "),bufsize:("
    //            << sdk_data_ptr->depthImg.bufferSize << "),frame id:("
    //            << sdk_data_ptr->depthImg.frameId << "),ts:("
    //            << sdk_data_ptr->depthImg.ts << "),("
    //            << camera_ptr->parameter_.cxir << ","
    //            << camera_ptr->parameter_.cyir << ","
    //            << camera_ptr->parameter_.fxir << ","
    //            << camera_ptr->parameter_.fyir << ")";
    // LOG(ERROR) << camera_ptr->name_ << " irImg size:("
    //            << sdk_data_ptr->irImg.size << "),pointCloud2 size:("
    //            << sdk_data_ptr->pointCloud2.size() << "),pointCloud size:("
    //            << sdk_data_ptr->pointCloud.size << ")";
    size_t width = sdk_data_ptr->depthImg.width;
    size_t height = sdk_data_ptr->depthImg.height;
    uint16_t *data_ptr = static_cast<uint16_t *>(sdk_data_ptr->depthImg.data);
    std::vector<Eigen::Vector3f> clouds;
    clouds.reserve(width * height);
    static Eigen::Vector3f last(0.0f, 0.0f, 0.0f);

    for (size_t i = kCameraSampleSize / 2; i < width; i += kCameraSampleSize) {
        for (size_t j = kCameraSampleSize / 2; j < height;
             j += kCameraSampleSize) {
            // LOG(ERROR) << "depth:(" << depth << ")";
            // uint16_t depth = *(data_ptr + (j * width + i));

            // LOG(ERROR) << "i,j:(" << i << "," << j << ")";
            float z = float(*(data_ptr + (j * width + i))) / 1000;
            size_t interval = 0;
            if (z < 0.1 || z > 8.0) {
                continue;
            } else if (z >= 0.1 && z < 1.0) {
                interval = 4;
            } else if (z >= 1.0 && z < 2.0) {
                interval = 2;
            } else if (z >= 2.0) {
                interval = 1;
            }
            LOG(ERROR) << "z:(" << z << "),interval:(" << interval << ").";
            for (size_t u = i - kCameraSampleSize / 2;
                 u < i + kCameraSampleSize / 2; u += interval) {
                for (size_t v = j - kCameraSampleSize / 2;
                     v < j + kCameraSampleSize / 2; v += interval) {
                    z = float(*(data_ptr + (j * width + i))) / 1000;
                    Eigen::Vector3f point(((i - camera_ptr->parameter_.cxir) *
                                           z / camera_ptr->parameter_.fxir),
                                          ((j - camera_ptr->parameter_.cyir) *
                                           z / camera_ptr->parameter_.fyir),
                                          z);
                    clouds.emplace_back(point);
                    LOG(ERROR) << "u,v:(" << u << "," << v << ")";
                    LOG(ERROR) << "i:(" << i << "),z:(" << z << "),interval:("
                               << (point - last).norm() << ").";
                    last = point;
                }
            }

            // LOG(ERROR) << "i:(" << i << "),z:(" << z << ")";
            // Eigen::Vector3f point(((i - camera_ptr->parameter_.cxir) * z /
            //                        camera_ptr->parameter_.fxir) /
            //                           1000,
            //                       ((j - camera_ptr->parameter_.cyir) * z /
            //                        camera_ptr->parameter_.fyir) /
            //                           1000,
            //                       z);
            // clouds.emplace_back(point);
            // // LOG(ERROR) << "i:(" << i << "),z:(" << z << "),interval:("
            // //            << (point - last).norm() << ").";
            // // i = i + size_t(z / 0.3);
            // last = point;
        }
    }
    LOG(ERROR) << "Raw size:(" << width * height << "),Clouds size:("
               << clouds.size() << ").";
    // std::time_t timestamp(sdk_data_ptr->depthImg.ts);//ros
    // now:(1723615000.770410004),ts:(1723615000765) auto milli = timestamp
    // + (long long)8 * 60 * 60 * 1000; auto mTime =
    // std::chrono::milliseconds(milli); auto tp =
    // std::chrono::time_point<std::chrono::system_clock,
    //                                   std::chrono::milliseconds>(mTime);
    // auto tt = std::chrono::system_clock::to_time_t(tp);
    // std::tm *now = std::gmtime(&tt);
    // printf("%4d年%02d月%02d日 %02d:%02d:%02d\n", now->tm_year + 1900,
    //        now->tm_mon + 1, now->tm_mday, now->tm_hour, now->tm_min,
    //        now->tm_sec);
    // LOG(ERROR) << "ros now:(" << ros::Time::now() << "),ts:("
    //            << sdk_data_ptr->depthImg.ts << ")";
}

bool Camera::SwitchCallBack(std_srvs::SetBool::Request &req,
                            std_srvs::SetBool::Response &res) {
    if (SetRunning(req.data)) {
        res.success = true;
        return true;
    } else {
        res.success = false;
        return false;
    }
}